#include "zf_common_headfile.h"
#include "motor.h"

extern float speed_l_now,speed_r_now;
//编码器测试用标志位
bool test_10ms_flag;
/**
 * @brief 电机初始化/PWM初始化
 * @date   2023-4-4
 * @param freq PWM周期
 **/
void motor_init(uint16_t freq)
{
    pwm_init(TIM8_PWM_CH1_C6,freq,0);
    pwm_init(TIM8_PWM_CH2_C7,freq,0);
    pwm_init(TIM8_PWM_CH3_C8,freq,0);
    pwm_init(TIM8_PWM_CH4_C9,freq,0);
    
    encoder_quad_init(TIM3_ENCODER, TIM3_ENCODER_CH1_B4, TIM3_ENCODER_CH2_B5);
    encoder_quad_init(TIM4_ENCODER, TIM4_ENCODER_CH1_B6, TIM4_ENCODER_CH2_B7);
}

/**
 * @brief 电机速度设定
 * @date  2023-4-4
 * @param speed_l_set 左电机的设定转速
 * @param speed_r_set 右电机的设定速度
 **/
#define speedl_dead 350
#define speedr_dead 340
void motor_speedset(float speed_l_set,float speed_r_set)
{    
    if(speed_l_set>0)
    {
        pwm_set_duty(TIM8_PWM_CH1_C6,speed_l_set+speedl_dead);
        pwm_set_duty(TIM8_PWM_CH2_C7,0);
    }
    else if (speed_l_set<0)
    {
        pwm_set_duty(TIM8_PWM_CH1_C6,0);
        pwm_set_duty(TIM8_PWM_CH2_C7,-speed_l_set+speedl_dead);
    }
    else 
    {
        pwm_set_duty(TIM8_PWM_CH1_C6,0);
        pwm_set_duty(TIM8_PWM_CH2_C7,0);
    }
    
        if(speed_r_set>0)
    {
        pwm_set_duty(TIM8_PWM_CH3_C8,0);
        pwm_set_duty(TIM8_PWM_CH4_C9,speed_r_set+speedr_dead);
    }
    else if (speed_r_set<0)
    {
        pwm_set_duty(TIM8_PWM_CH3_C8,-speed_r_set+speedr_dead);
        pwm_set_duty(TIM8_PWM_CH4_C9,0);
    }
    else 
    {
        pwm_set_duty(TIM8_PWM_CH3_C8,0);
        pwm_set_duty(TIM8_PWM_CH4_C9,0);
    }
}

/*
 * @brief 编码器读取左右电机的转速
 * @date  2023-4-4 
 * @note  得到的值传给speed_l_now与speed_r_now
 */
#define SPEED_RECORD_NUM 20 // 经测试，50Hz个采样值进行滤波的效果比较好
float speed_l_Record[SPEED_RECORD_NUM]={0};
float speed_r_Record[SPEED_RECORD_NUM]={0};
/*
 * 进行速度的平均滤波
 * 输入新采样到的速度，存放速度的数组，
 * 返回滤波后的速度
 */
float Speed_Low_Filter(float new_Spe,float *speed_Record)
{
    float sum = 0.0f;
    for(uint8_t i=SPEED_RECORD_NUM-1;i>0;i--)//将现有数据后移一位
    {
        speed_Record[i] = speed_Record[i-1];
        sum += speed_Record[i-1];
    }
    speed_Record[0] = new_Spe;//第一位是新的数据
    sum += new_Spe;
    return sum/SPEED_RECORD_NUM;//返回均值
}

void read_speed(void)
{
    float temp,temp2;
    //temp=encoder_get_count(TIM3_ENCODER)/1024*(30/68)*(6.4*3.1415)/0.005 cm/s 1.7324 0.8662
    temp=encoder_get_count(TIM3_ENCODER)*1.7324;
    encoder_clear_count(TIM3_ENCODER);
    speed_l_now=Speed_Low_Filter(temp,speed_l_Record);
    
    temp2=(-encoder_get_count(TIM4_ENCODER))*1.7324;
    encoder_clear_count(TIM4_ENCODER);
    speed_r_now=Speed_Low_Filter(temp2,speed_r_Record);
    
}

/**
 * @brief 测试编码器
 * @date  2023-4-4
 * @note  得到的值未正负处理通过串口打印，需要检查isr.c文件里面的标志位是否被注释
 * @note  目前设定为10ms读一次
 */
void test_encoder(void)
{
    char txt[60];
    encoder_quad_init(TIM3_ENCODER, TIM3_ENCODER_CH1_B4, TIM3_ENCODER_CH2_B5);
    encoder_quad_init(TIM4_ENCODER, TIM4_ENCODER_CH1_B6, TIM4_ENCODER_CH2_B7);
    pit_ms_init(TIM2_PIT,10);
    while(1)
    {
        if(test_10ms_flag)
        {
        printf("L=%d,R=%d\r\n",encoder_get_count(TIM3_ENCODER),encoder_get_count(TIM4_ENCODER));
        encoder_clear_count(TIM3_ENCODER);
        encoder_clear_count(TIM4_ENCODER);
        test_10ms_flag=0;
        }
    }
}

/**
 * @brief 测试电机函数，功能为通过串口设定速度，智能车可以按照该速度设定PWM，同时通过编码器输出测量的速度
 * @date  2023-4-4
 * @note 使用UART5作为通信串口
 **/
uint8_t uart1_get_data[64];
uint8_t fifo_get_data[64];//缓冲区

uint8_t get_data ;
uint32_t fifo_data_count ;

fifo_struct uart1_data_fifo;

void test_motor(void)
{
    char txt[60];
    int16_t basic_speed=0;
    int32_t temp=0,temp2=0,temp3=0;
    uint16_t time;
    encoder_quad_init(TIM3_ENCODER, TIM3_ENCODER_CH1_B4, TIM3_ENCODER_CH2_B5);
    encoder_quad_init(TIM4_ENCODER, TIM4_ENCODER_CH1_B6, TIM4_ENCODER_CH2_B7);
    motor_init(17000);
    tft180_init();
    fifo_init(&uart1_data_fifo,FIFO_DATA_8BIT,uart1_get_data,64);
    uart_init(UART_1,115200, UART1_TX_A9, UART1_RX_A10  );
    uart_rx_interrupt(UART_1,1);
    interrupt_set_priority(UART1_IRQn,2);
    pit_ms_init(TIM2_PIT,5);
    interrupt_set_priority(TIM2_IRQn,3);
    gpio_init(C10, GPO, GPIO_HIGH, GPO_PUSH_PULL);
    
    while(1)
    {
        if(test_10ms_flag)
        {
        gpio_set_level(C10,1);
        fifo_data_count = fifo_used(&uart1_data_fifo);                      // 查看 fifo 是否有数据
        if(0 != fifo_data_count)                                                // 读取到数据了
        {
            fifo_read_buffer(&uart1_data_fifo, fifo_get_data, &fifo_data_count, FIFO_READ_AND_CLEAN);    // 将 fifo 中数据读出并清空 fifo 挂载的缓冲

            if(strstr((char*)fifo_get_data,"left"))
            {
                sscanf((char*)fifo_get_data,"left:%05d\r\n",&temp);
            tft180_show_int(0,32,temp,5);
            }
             if(strstr((char*)fifo_get_data,"right"))
            {
                sscanf((char*)fifo_get_data,"right:%05d\r\n",&temp2);
            tft180_show_int(0,0,temp2,5); 
            }
            memset(fifo_get_data,0,sizeof(fifo_get_data));
            
            motor_speedset(temp,temp2);
        }

        read_speed();
        sprintf(txt,"%d,%d,%.2f,%.2f\r\n",temp,temp2,speed_l_now,speed_r_now);//vofa firewater协议
        uart_write_string(UART_1,txt);
        gpio_set_level(C10,0);
        test_10ms_flag=0;
        }
    }
}

/**
 * @brief串口5接受中断服务函数
 * @note 给test_motor函数服务用 
 * @date 2023-4-4
 */
void uart1_rx_interrupt_handler(void)
{
    uart_query_byte(UART_1, &get_data); // 接收数据 查询式 有数据会返回 TRUE 没有数据会返回 FALSE
    fifo_write_buffer(&uart1_data_fifo, &get_data, 1);  //一次读一个
}


